//
#include "tsp.h"
#include <iostream>

CFAR::CFAR(const std::string& pull_address, const std::string& rep_address)
    : pull_address(pull_address), rep_address(rep_address) {
    pull_socket = zmq::socket_t(context, ZMQ_PULL);
    pull_socket.connect(pull_address);
    
    rep_socket = zmq::socket_t(context, ZMQ_REP);
    rep_socket.bind(rep_address);
}

CFAR::~CFAR() {
    pull_socket.close();
    rep_socket.close();
}

void CFAR::startup() {
    while (true) {
        // 处理CFAR控制命令
        zmq::message_t command;
        try {
            rep_socket.recv(command, zmq::recv_flags::none);
        } catch (const zmq::error_t& e) {
            std::cerr << "ZMQ error: " << e.what() << std::endl;
            continue;
        }
        
        float new_threshold;
        memcpy(&new_threshold, command.data(), sizeof(float));
        setThreshold(new_threshold);
        
        // 发送确认
        zmq::message_t ack(sizeof(float));
        memcpy(ack.data(), &threshold, sizeof(float));
        rep_socket.send(ack, zmq::send_flags::none);
        std::cout << "    cfar.threshold=" << threshold << "; ???????????" << std::endl;
        
        // 处理信号
        zmq::message_t message;
        try {
            pull_socket.recv(message, zmq::recv_flags::none);
        } catch (const zmq::error_t& e) {
            std::cerr << "ZMQ error: " << e.what() << std::endl;
            continue;
        }
        
        float* input = static_cast<float*>(message.data());
        float output[1024];
        
        // performCFAR(input, output, 1024);
        
        zmq::message_t response(sizeof(float) * 1024);
        memcpy(response.data(), output, sizeof(float) * 1024);
        
        try {
            // 这里应该发送到下一个模块，但示例中省略了
            // push_socket.send(response, zmq::send_flags::none);
        } catch (const zmq::error_t& e) {
            std::cerr << "ZMQ error: " << e.what() << std::endl;
        }
    }
}

void CFAR::setThreshold(float threshold) {
    this->threshold = threshold;
}

void CFAR::performCFAR(float* input, float* output, size_t size) {
    // 简化的CA-CFAR算法示例
    for (size_t i = 0; i < size; ++i) {
        output[i] = input[i] > threshold ? input[i] : 0;
    }
}